#include "DAP_config.h"
#include "DAP.h"

//********************************************************************************
// SW Macros
#define PIN_SWCLK_SET_1 PIN_SWCLK_TCK_SET_1
#define PIN_SWCLK_CLR_1 PIN_SWCLK_TCK_CLR_1

#define SW_CLOCK_CYCLE_1()                \
  PIN_SWCLK_CLR_1();                      \
  PIN_DELAY_1();                          \
  PIN_SWCLK_SET_1();                      \
  PIN_DELAY_1()

#define SW_WRITE_BIT_1(bit)               \
  PIN_SWDIO_OUT_1(bit);                   \
  PIN_SWCLK_CLR_1();                      \
  PIN_DELAY_1();                          \
  PIN_SWCLK_SET_1();                      \
  PIN_DELAY_1()

#define SW_READ_BIT_1(bit)                \
  PIN_SWCLK_CLR_1();                      \
  PIN_DELAY_1();                          \
  bit = PIN_SWDIO_IN_1();                 \
  PIN_SWCLK_SET_1();                      \
  PIN_DELAY_1()

#define PIN_DELAY_1() PIN_DELAY_SLOW(DAP_Data_1.clock_delay)
//#define PIN_DELAY_1() PIN_DELAY_FAST()

// Generate SWD Sequence
//   count:  sequence bit count
//   data:   pointer to sequence bit data
//   return: none
void SWD_Sequence_1 (uint32_t count, const uint8_t *data) {
  uint32_t val;
  uint32_t n;

  val = 0U;
  n = 0U;
  while (count--) {
    if (n == 0U) {
      val = *data++;
      n = 8U;
    }
    if (val & 1U) {
      PIN_SWDIO_TMS_SET_1();
    } else {
      PIN_SWDIO_TMS_CLR_1();
    }
    SW_CLOCK_CYCLE_1();
    val >>= 1;
    n--;
  }
}

// SWD Transfer I/O
//   request: A[3:2] RnW APnDP
//   data:    DATA[31:0]
//   return:  ACK[2:0]
static uint8_t SWD_Transfer_Buff_1(uint32_t request, uint32_t *data) {         
  uint32_t ack;                                                                 
  uint32_t bit;                                                                 
  uint32_t val;                                                                 
  uint32_t parity;                                                              
                                                                                
  uint32_t n;                                                                   
                                                                                
  /* Packet Request */                                                          
  parity = 0U;                                                                  
  SW_WRITE_BIT_1(1U);                     /* Start Bit */                         
  bit = request >> 0;                                                           
  SW_WRITE_BIT_1(bit);                    /* APnDP Bit */                         
  parity += bit;                                                                
  bit = request >> 1;                                                           
  SW_WRITE_BIT_1(bit);                    /* RnW Bit */                           
  parity += bit;                                                                
  bit = request >> 2;                                                           
  SW_WRITE_BIT_1(bit);                    /* A2 Bit */                            
  parity += bit;                                                                
  bit = request >> 3;                                                           
  SW_WRITE_BIT_1(bit);                    /* A3 Bit */                            
  parity += bit;                                                                
  SW_WRITE_BIT_1(parity);                 /* Parity Bit */                        
  SW_WRITE_BIT_1(0U);                     /* Stop Bit */                          
  SW_WRITE_BIT_1(1U);                     /* Park Bit */                          
                                                                                
  /* Turnaround */                                                              
  PIN_SWDIO_OUT_DISABLE_1();                                                      
  for (n = DAP_Data_1.swd_conf.turnaround; n; n--) {                              
    SW_CLOCK_CYCLE_1();                                                           
  }                                                                             
                                                                                
  /* Acknowledge response */                                                    
  SW_READ_BIT_1(bit);                                                             
  ack  = bit << 0;                                                              
  SW_READ_BIT_1(bit);                                                             
  ack |= bit << 1;                                                              
  SW_READ_BIT_1(bit);                                                             
  ack |= bit << 2;                                                              
                                                                                
  if (ack == DAP_TRANSFER_OK) {         /* OK response */                       
    /* Data transfer */                                                         
    if (request & DAP_TRANSFER_RnW) {                                           
      /* Read data */                                                           
      val = 0U;                                                                 
      parity = 0U;                                                              
      for (n = 32U; n; n--) {                                                   
        SW_READ_BIT_1(bit);               /* Read RDATA[0:31] */                  
        parity += bit;                                                          
        val >>= 1;                                                              
        val  |= bit << 31;                                                      
      }                                                                         
      SW_READ_BIT_1(bit);                 /* Read Parity */                       
      if ((parity ^ bit) & 1U) {                                                
        ack = DAP_TRANSFER_ERROR;                                               
      }                                                                         
      if (data) { *data = val; }                                                
      /* Turnaround */                                                          
      for (n = DAP_Data_1.swd_conf.turnaround; n; n--) {                          
        SW_CLOCK_CYCLE_1();                                                       
      }                                                                         
      PIN_SWDIO_OUT_ENABLE_1();                                                   
    } else {                                                                    
      /* Turnaround */                                                          
      for (n = DAP_Data_1.swd_conf.turnaround; n; n--) {                          
        SW_CLOCK_CYCLE_1();                                                       
      }                                                                         
      PIN_SWDIO_OUT_ENABLE_1();                                                   
      /* Write data */                                                          
      val = *data;                                                              
      parity = 0U;                                                              
      for (n = 32U; n; n--) {                                                   
        SW_WRITE_BIT_1(val);              /* Write WDATA[0:31] */                 
        parity += val;                                                          
        val >>= 1;                                                              
      }                                                                         
      SW_WRITE_BIT_1(parity);             /* Write Parity Bit */                  
    }                                                                           
    /* Capture Timestamp */                                                     
    if (request & DAP_TRANSFER_TIMESTAMP) {                                     
      DAP_Data_1.timestamp = TIMESTAMP_GET();                                     
    }                                                                           
    /* Idle cycles */                                                           
    n = DAP_Data_1.transfer.idle_cycles;                                          
    if (n) {                                                                    
      PIN_SWDIO_OUT_1(0U);                                                        
      for (; n; n--) {                                                          
        SW_CLOCK_CYCLE_1();                                                       
      }                                                                         
    }                                                                           
    PIN_SWDIO_OUT_1(1U);                                                          
    return ((uint8_t)ack);                                                      
  }                                                                             
                                                                                
  if ((ack == DAP_TRANSFER_WAIT) || (ack == DAP_TRANSFER_FAULT)) {              
    /* WAIT or FAULT response */                                                
    if (DAP_Data_1.swd_conf.data_phase && ((request & DAP_TRANSFER_RnW) != 0U)) { 
      for (n = 32U+1U; n; n--) {                                                
        SW_CLOCK_CYCLE_1();               /* Dummy Read RDATA[0:31] + Parity */   
      }                                                                         
    }                                                                           
    /* Turnaround */                                                            
    for (n = DAP_Data_1.swd_conf.turnaround; n; n--) {                            
      SW_CLOCK_CYCLE_1();                                                         
    }                                                                           
    PIN_SWDIO_OUT_ENABLE_1();                                                     
    if (DAP_Data_1.swd_conf.data_phase && ((request & DAP_TRANSFER_RnW) == 0U)) { 
      PIN_SWDIO_OUT_1(0U);                                                        
      for (n = 32U+1U; n; n--) {                                                
        SW_CLOCK_CYCLE_1();               /* Dummy Write WDATA[0:31] + Parity */  
      }                                                                         
    }                                                                           
    PIN_SWDIO_OUT_1(1U);                                                          
    return ((uint8_t)ack);                                                      
  }                                                                             
                                                                                
  /* Protocol error */                                                          
  for (n = DAP_Data_1.swd_conf.turnaround + 32U + 1U; n; n--) {                   
    SW_CLOCK_CYCLE_1();                   /* Back off data phase */               
  }                                                                             
  PIN_SWDIO_OUT_ENABLE_1();                                                       
  PIN_SWDIO_OUT_1(1U);                                                            
  return ((uint8_t)ack);                                                        
}

// SWD Transfer I/O
//   request: A[3:2] RnW APnDP
//   data:    DATA[31:0]
//   return:  ACK[2:0]
uint8_t  SWD_Transfer_1(uint32_t request, uint32_t *data) {
	return SWD_Transfer_Buff_1(request,data); 
}

//********************************************************************************

// SW Macros
#define PIN_SWCLK_SET_2 PIN_SWCLK_TCK_SET_2
#define PIN_SWCLK_CLR_2 PIN_SWCLK_TCK_CLR_2

#define SW_CLOCK_CYCLE_2()                \
  PIN_SWCLK_CLR_2();                      \
  PIN_DELAY_2();                          \
  PIN_SWCLK_SET_2();                      \
  PIN_DELAY_2()

#define SW_WRITE_BIT_2(bit)               \
  PIN_SWDIO_OUT_2(bit);                   \
  PIN_SWCLK_CLR_2();                      \
  PIN_DELAY_2();                          \
  PIN_SWCLK_SET_2();                      \
  PIN_DELAY_2()

#define SW_READ_BIT_2(bit)                \
  PIN_SWCLK_CLR_2();                      \
  PIN_DELAY_2();                          \
  bit = PIN_SWDIO_IN_2();                 \
  PIN_SWCLK_SET_2();                      \
  PIN_DELAY_2()

#define PIN_DELAY_2() PIN_DELAY_SLOW(DAP_Data_2.clock_delay)
//#define PIN_DELAY_2() PIN_DELAY_FAST()

// Generate SWD Sequence
//   count:  sequence bit count
//   data:   pointer to sequence bit data
//   return: none
void SWD_Sequence_2 (uint32_t count, const uint8_t *data) {
  uint32_t val;
  uint32_t n;

  val = 0U;
  n = 0U;
  while (count--) {
    if (n == 0U) {
      val = *data++;
      n = 8U;
    }
    if (val & 1U) {
      PIN_SWDIO_TMS_SET_2();
    } else {
      PIN_SWDIO_TMS_CLR_2();
    }
    SW_CLOCK_CYCLE_2();
    val >>= 1;
    n--;
  }
}

// SWD Transfer I/O
//   request: A[3:2] RnW APnDP
//   data:    DATA[31:0]
//   return:  ACK[2:0]
static uint8_t SWD_Transfer_Buff_2(uint32_t request, uint32_t *data) {         
  uint32_t ack;                                                                 
  uint32_t bit;                                                                 
  uint32_t val;                                                                 
  uint32_t parity;                                                              
                                                                                
  uint32_t n;                                                                   
                                                                                
  /* Packet Request */                                                          
  parity = 0U;                                                                  
  SW_WRITE_BIT_2(1U);                     /* Start Bit */                         
  bit = request >> 0;                                                           
  SW_WRITE_BIT_2(bit);                    /* APnDP Bit */                         
  parity += bit;                                                                
  bit = request >> 1;                                                           
  SW_WRITE_BIT_2(bit);                    /* RnW Bit */                           
  parity += bit;                                                                
  bit = request >> 2;                                                           
  SW_WRITE_BIT_2(bit);                    /* A2 Bit */                            
  parity += bit;                                                                
  bit = request >> 3;                                                           
  SW_WRITE_BIT_2(bit);                    /* A3 Bit */                            
  parity += bit;                                                                
  SW_WRITE_BIT_2(parity);                 /* Parity Bit */                        
  SW_WRITE_BIT_2(0U);                     /* Stop Bit */                          
  SW_WRITE_BIT_2(1U);                     /* Park Bit */                          
                                                                                
  /* Turnaround */                                                              
  PIN_SWDIO_OUT_DISABLE_2();                                                      
  for (n = DAP_Data_2.swd_conf.turnaround; n; n--) {                              
    SW_CLOCK_CYCLE_2();                                                           
  }                                                                             
                                                                                
  /* Acknowledge response */                                                    
  SW_READ_BIT_2(bit);                                                             
  ack  = bit << 0;                                                              
  SW_READ_BIT_2(bit);                                                             
  ack |= bit << 1;                                                              
  SW_READ_BIT_2(bit);                                                             
  ack |= bit << 2;                                                              
                                                                                
  if (ack == DAP_TRANSFER_OK) {         /* OK response */                       
    /* Data transfer */                                                         
    if (request & DAP_TRANSFER_RnW) {                                           
      /* Read data */                                                           
      val = 0U;                                                                 
      parity = 0U;                                                              
      for (n = 32U; n; n--) {                                                   
        SW_READ_BIT_2(bit);               /* Read RDATA[0:31] */                  
        parity += bit;                                                          
        val >>= 1;                                                              
        val  |= bit << 31;                                                      
      }                                                                         
      SW_READ_BIT_2(bit);                 /* Read Parity */                       
      if ((parity ^ bit) & 1U) {                                                
        ack = DAP_TRANSFER_ERROR;                                               
      }                                                                         
      if (data) { *data = val; }                                                
      /* Turnaround */                                                          
      for (n = DAP_Data_2.swd_conf.turnaround; n; n--) {                          
        SW_CLOCK_CYCLE_2();                                                       
      }                                                                         
      PIN_SWDIO_OUT_ENABLE_2();                                                   
    } else {                                                                    
      /* Turnaround */                                                          
      for (n = DAP_Data_2.swd_conf.turnaround; n; n--) {                          
        SW_CLOCK_CYCLE_2();                                                       
      }                                                                         
      PIN_SWDIO_OUT_ENABLE_2();                                                   
      /* Write data */                                                          
      val = *data;                                                              
      parity = 0U;                                                              
      for (n = 32U; n; n--) {                                                   
        SW_WRITE_BIT_2(val);              /* Write WDATA[0:31] */                 
        parity += val;                                                          
        val >>= 1;                                                              
      }                                                                         
      SW_WRITE_BIT_2(parity);             /* Write Parity Bit */                  
    }                                                                           
    /* Capture Timestamp */                                                     
    if (request & DAP_TRANSFER_TIMESTAMP) {                                     
      DAP_Data_2.timestamp = TIMESTAMP_GET();                                     
    }                                                                           
    /* Idle cycles */                                                           
    n = DAP_Data_2.transfer.idle_cycles;                                          
    if (n) {                                                                    
      PIN_SWDIO_OUT_2(0U);                                                        
      for (; n; n--) {                                                          
        SW_CLOCK_CYCLE_2();                                                       
      }                                                                         
    }                                                                           
    PIN_SWDIO_OUT_2(1U);                                                          
    return ((uint8_t)ack);                                                      
  }                                                                             
                                                                                
  if ((ack == DAP_TRANSFER_WAIT) || (ack == DAP_TRANSFER_FAULT)) {              
    /* WAIT or FAULT response */                                                
    if (DAP_Data_2.swd_conf.data_phase && ((request & DAP_TRANSFER_RnW) != 0U)) { 
      for (n = 32U+1U; n; n--) {                                                
        SW_CLOCK_CYCLE_2();               /* Dummy Read RDATA[0:31] + Parity */   
      }                                                                         
    }                                                                           
    /* Turnaround */                                                            
    for (n = DAP_Data_2.swd_conf.turnaround; n; n--) {                            
      SW_CLOCK_CYCLE_2();                                                         
    }                                                                           
    PIN_SWDIO_OUT_ENABLE_2();                                                     
    if (DAP_Data_2.swd_conf.data_phase && ((request & DAP_TRANSFER_RnW) == 0U)) { 
      PIN_SWDIO_OUT_2(0U);                                                        
      for (n = 32U+1U; n; n--) {                                                
        SW_CLOCK_CYCLE_2();               /* Dummy Write WDATA[0:31] + Parity */  
      }                                                                         
    }                                                                           
    PIN_SWDIO_OUT_2(1U);                                                          
    return ((uint8_t)ack);                                                      
  }                                                                             
                                                                                
  /* Protocol error */                                                          
  for (n = DAP_Data_2.swd_conf.turnaround + 32U + 1U; n; n--) {                   
    SW_CLOCK_CYCLE_2();                   /* Back off data phase */               
  }                                                                             
  PIN_SWDIO_OUT_ENABLE_2();                                                       
  PIN_SWDIO_OUT_2(1U);                                                            
  return ((uint8_t)ack);                                                        
}

// SWD Transfer I/O
//   request: A[3:2] RnW APnDP
//   data:    DATA[31:0]
//   return:  ACK[2:0]
uint8_t  SWD_Transfer_2(uint32_t request, uint32_t *data) {
	return SWD_Transfer_Buff_2(request,data); 
}

//********************************************************************************
// SW Macros
#define PIN_SWCLK_SET_3 PIN_SWCLK_TCK_SET_3
#define PIN_SWCLK_CLR_3 PIN_SWCLK_TCK_CLR_3

#define SW_CLOCK_CYCLE_3()                \
  PIN_SWCLK_CLR_3();                      \
  PIN_DELAY_3();                          \
  PIN_SWCLK_SET_3();                      \
  PIN_DELAY_3()

#define SW_WRITE_BIT_3(bit)               \
  PIN_SWDIO_OUT_3(bit);                   \
  PIN_SWCLK_CLR_3();                      \
  PIN_DELAY_3();                          \
  PIN_SWCLK_SET_3();                      \
  PIN_DELAY_3()

#define SW_READ_BIT_3(bit)                \
  PIN_SWCLK_CLR_3();                      \
  PIN_DELAY_3();                          \
  bit = PIN_SWDIO_IN_3();                 \
  PIN_SWCLK_SET_3();                      \
  PIN_DELAY_3()

#define PIN_DELAY_3() PIN_DELAY_SLOW(DAP_Data_3.clock_delay)
//#define PIN_DELAY_3() PIN_DELAY_FAST()

// Generate SWD Sequence
//   count:  sequence bit count
//   data:   pointer to sequence bit data
//   return: none
void SWD_Sequence_3 (uint32_t count, const uint8_t *data) {
  uint32_t val;
  uint32_t n;

  val = 0U;
  n = 0U;
  while (count--) {
    if (n == 0U) {
      val = *data++;
      n = 8U;
    }
    if (val & 1U) {
      PIN_SWDIO_TMS_SET_3();
    } else {
      PIN_SWDIO_TMS_CLR_3();
    }
    SW_CLOCK_CYCLE_3();
    val >>= 1;
    n--;
  }
}

// SWD Transfer I/O
//   request: A[3:2] RnW APnDP
//   data:    DATA[31:0]
//   return:  ACK[2:0]
static uint8_t SWD_Transfer_Buff_3(uint32_t request, uint32_t *data) {         
  uint32_t ack;                                                                 
  uint32_t bit;                                                                 
  uint32_t val;                                                                 
  uint32_t parity;                                                              
                                                                                
  uint32_t n;                                                                   
                                                                                
  /* Packet Request */                                                          
  parity = 0U;                                                                  
  SW_WRITE_BIT_3(1U);                     /* Start Bit */                         
  bit = request >> 0;                                                           
  SW_WRITE_BIT_3(bit);                    /* APnDP Bit */                         
  parity += bit;                                                                
  bit = request >> 1;                                                           
  SW_WRITE_BIT_3(bit);                    /* RnW Bit */                           
  parity += bit;                                                                
  bit = request >> 2;                                                           
  SW_WRITE_BIT_3(bit);                    /* A2 Bit */                            
  parity += bit;                                                                
  bit = request >> 3;                                                           
  SW_WRITE_BIT_3(bit);                    /* A3 Bit */                            
  parity += bit;                                                                
  SW_WRITE_BIT_3(parity);                 /* Parity Bit */                        
  SW_WRITE_BIT_3(0U);                     /* Stop Bit */                          
  SW_WRITE_BIT_3(1U);                     /* Park Bit */                          
                                                                                
  /* Turnaround */                                                              
  PIN_SWDIO_OUT_DISABLE_3();                                                      
  for (n = DAP_Data_3.swd_conf.turnaround; n; n--) {                              
    SW_CLOCK_CYCLE_3();                                                           
  }                                                                             
                                                                                
  /* Acknowledge response */                                                    
  SW_READ_BIT_3(bit);                                                             
  ack  = bit << 0;                                                              
  SW_READ_BIT_3(bit);                                                             
  ack |= bit << 1;                                                              
  SW_READ_BIT_3(bit);                                                             
  ack |= bit << 2;                                                              
                                                                                
  if (ack == DAP_TRANSFER_OK) {         /* OK response */                       
    /* Data transfer */                                                         
    if (request & DAP_TRANSFER_RnW) {                                           
      /* Read data */                                                           
      val = 0U;                                                                 
      parity = 0U;                                                              
      for (n = 32U; n; n--) {                                                   
        SW_READ_BIT_3(bit);               /* Read RDATA[0:31] */                  
        parity += bit;                                                          
        val >>= 1;                                                              
        val  |= bit << 31;                                                      
      }                                                                         
      SW_READ_BIT_3(bit);                 /* Read Parity */                       
      if ((parity ^ bit) & 1U) {                                                
        ack = DAP_TRANSFER_ERROR;                                               
      }                                                                         
      if (data) { *data = val; }                                                
      /* Turnaround */                                                          
      for (n = DAP_Data_3.swd_conf.turnaround; n; n--) {                          
        SW_CLOCK_CYCLE_3();                                                       
      }                                                                         
      PIN_SWDIO_OUT_ENABLE_3();                                                   
    } else {                                                                    
      /* Turnaround */                                                          
      for (n = DAP_Data_3.swd_conf.turnaround; n; n--) {                          
        SW_CLOCK_CYCLE_3();                                                       
      }                                                                         
      PIN_SWDIO_OUT_ENABLE_3();                                                   
      /* Write data */                                                          
      val = *data;                                                              
      parity = 0U;                                                              
      for (n = 32U; n; n--) {                                                   
        SW_WRITE_BIT_3(val);              /* Write WDATA[0:31] */                 
        parity += val;                                                          
        val >>= 1;                                                              
      }                                                                         
      SW_WRITE_BIT_3(parity);             /* Write Parity Bit */                  
    }                                                                           
    /* Capture Timestamp */                                                     
    if (request & DAP_TRANSFER_TIMESTAMP) {                                     
      DAP_Data_3.timestamp = TIMESTAMP_GET();                                     
    }                                                                           
    /* Idle cycles */                                                           
    n = DAP_Data_3.transfer.idle_cycles;                                          
    if (n) {                                                                    
      PIN_SWDIO_OUT_3(0U);                                                        
      for (; n; n--) {                                                          
        SW_CLOCK_CYCLE_3();                                                       
      }                                                                         
    }                                                                           
    PIN_SWDIO_OUT_3(1U);                                                          
    return ((uint8_t)ack);                                                      
  }                                                                             
                                                                                
  if ((ack == DAP_TRANSFER_WAIT) || (ack == DAP_TRANSFER_FAULT)) {              
    /* WAIT or FAULT response */                                                
    if (DAP_Data_3.swd_conf.data_phase && ((request & DAP_TRANSFER_RnW) != 0U)) { 
      for (n = 32U+1U; n; n--) {                                                
        SW_CLOCK_CYCLE_3();               /* Dummy Read RDATA[0:31] + Parity */   
      }                                                                         
    }                                                                           
    /* Turnaround */                                                            
    for (n = DAP_Data_3.swd_conf.turnaround; n; n--) {                            
      SW_CLOCK_CYCLE_3();                                                         
    }                                                                           
    PIN_SWDIO_OUT_ENABLE_3();                                                     
    if (DAP_Data_3.swd_conf.data_phase && ((request & DAP_TRANSFER_RnW) == 0U)) { 
      PIN_SWDIO_OUT_3(0U);                                                        
      for (n = 32U+1U; n; n--) {                                                
        SW_CLOCK_CYCLE_3();               /* Dummy Write WDATA[0:31] + Parity */  
      }                                                                         
    }                                                                           
    PIN_SWDIO_OUT_3(1U);                                                          
    return ((uint8_t)ack);                                                      
  }                                                                             
                                                                                
  /* Protocol error */                                                          
  for (n = DAP_Data_3.swd_conf.turnaround + 32U + 1U; n; n--) {                   
    SW_CLOCK_CYCLE_3();                   /* Back off data phase */               
  }                                                                             
  PIN_SWDIO_OUT_ENABLE_3();                                                       
  PIN_SWDIO_OUT_3(1U);                                                            
  return ((uint8_t)ack);                                                        
}

// SWD Transfer I/O
//   request: A[3:2] RnW APnDP
//   data:    DATA[31:0]
//   return:  ACK[2:0]
uint8_t  SWD_Transfer_3(uint32_t request, uint32_t *data) {
	return SWD_Transfer_Buff_3(request,data); 
}

//********************************************************************************
